package org.usfirst.frc.team1322.robot.subsystems;

import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.VictorSP;
import org.usfirst.frc.team1322.robot.RobotMap;
import edu.wpi.first.wpilibj.DigitalInput;
import org.usfirst.frc.team1322.robot.commands.*;

/**
 *	TODO: Verify Operation
 */
public class LiftSystem extends Subsystem {
    
    // Put methods for controlling this subsystem
    // here. Call these from Commands.
	VictorSP mLift, mLiftPositionScrew;
	DigitalInput mLiftLowerLimit, mLiftUpperLimit;
	private final double mPower = 1.0;	// Base Power
	 
	public LiftSystem(){
		mLift = new VictorSP(RobotMap.pLift);
		mLiftPositionScrew = new VictorSP(RobotMap.pLiftPositionScrew);
		mLiftLowerLimit = new DigitalInput(RobotMap.pLiftLowerLimit);
		mLiftUpperLimit = new DigitalInput(RobotMap.pLiftUpperLimit);
	}
	
	public void raiseLift(){
		mLift.set(mPower);
	}
	
	public void lowerLift(){
		mLift.set(-mPower);
	}
	
	public void stopLift(){
		mLift.set(0);
	}
	
	public void liftPower(double mPow){
		mLift.set(mPow);
	}
	
	// TODO: Go to Pickup Position
	public void goToPickup(){
		
	}
	
	// TODO: Go to Place Position
	public void goToPlace(){
		
	}
	
	// TODO: Get Current Position of lift
	public int getPosition(){
		
		return 0;
	}
	
    public void initDefaultCommand() {
    	setDefaultCommand(new JoystickLift());
    }
    
}

